#include <p30F4011.h>
#include "settings.h"
#include "AC-Controller.h"
#include "globals.h"
#include "canbus.h"
#include "eeprom.h"

#include "acim_vhz.h" //for the global vars we'll be reading

void CANInit(void) {
	C1CTRLbits.REQOP = 0b100; 	        /* Request configuration mode */
	while(C1CTRLbits.OPMODE != 0b100);  /* Wait until configuration mode entered */
}


/*********************************************************************
* Function Name : CANConfig 
* Description   : Fcy is 16MHz.  Sets baud rate to 64Kbps with 25Tq
                         
*********************************************************************/
// Pg. 648
// Fcy could be a range of values. Luckily it is set in settings.h
//The goal is to create a scheme whereby various bauds can be selected solely by
//changing the baud rate prescaler. All values set here are actually one higher than
//they appear. So setting the prescaler to 4 really means 5. On top of that, the prescaler
//is always X2 so setting it to 4 really means (4+1) * 2 = 10
//Now, there is also a phantom 1 TQ start bit so add that too.
//Total TQ = 1 + (PRSEG + 1) + (SEG1PH + 1) + (SEG2PH + 1)
//or, if it's easier: 4 + PRSEG + SEG1PH + SEG2PH
//so start with 1M and try to find a good medium and work from there.
//0 = 64k baud
//1 = 125k baud
//2 = 250k baud
//3 = 500k baud
//4 = 1M baud
//the below code assumes the system clock to be 20MHz which it will be if you use a
//10Mhz crystal. So, do that.

void CANConfig(void) {
	int speed;
	speed = ee_get_canbus_speed();
	//speed = 3;
	// FRM Pg. 648
	C1CFG2bits.SAM = 0;       /* Bus is sampled once. */ 
	C1CFG1bits.SJW = 3;     //4 Tq   /* Synchronized jump width time is 4Tq. */
	C1CFG2bits.SEG2PHTS = 1;  /* PhaseSeg2 is freely programmable. */


	//The max speed is 1M so take the system clock and chew up enough TQ to get there.
	//the lowest prescaler for canbus is 2x so our 20M clock / 2 = 10M so we need ten time quanta
	//The prop is 3 and sync is 1 so that leaves 6 left to chew up.
	C1CFG2bits.PRSEG = 2;  //3TQ

	C1CFG2bits.SEG1PH = 2; //3
	C1CFG2bits.SEG2PH = 2; //3

	C1CFG1bits.BRP = speed;

	// Pg. 365
	C1RX0CONbits.DBEN = 1;		// if Receive Buffer 0 is full, it will go to Receive Buffer 1, and won't be lost.
	C1CTRL = 0b0000100000000000;  // CAN clock is Fcy. disable CAN capture. 
	while(C1CTRLbits.OPMODE != 0); // Wait for the current operating mode to be "Normal Operation Mode"
}

/*********************************************************************
* Function Name : CANSetFilter
* Description   : This function sets the acceptance filter, SID for 
                  standard length identifiers.

* Parameters    : uint16_t: filter_no:  (Filter number 0-5)
*                 uint16_t: sid:        (SID value)
*********************************************************************/

void CANSetFilter(uint8_t filter_no, uint16_t sid) {
	// There are 6 possible filters.  You load the 11 bit identifier into whichever filter the function caller specified.
	switch(filter_no) {
    	case 0: C1RXF0SID = 0; 		 // FRM Pg. 644
        	C1RXF0SIDbits.SID = sid; 
        break;
    	case 1: C1RXF1SID = 0;
            C1RXF1SIDbits.SID = sid; 
        break;
    	case 2: C1RXF2SID = 0;
            C1RXF2SIDbits.SID = sid; 
        break;
    	case 3: C1RXF3SID = 0;
            C1RXF3SIDbits.SID = sid; 
        break;
    	case 4: C1RXF4SID = 0;
            C1RXF4SIDbits.SID = sid; 
        break;
    	case 5: C1RXF5SID = 0;
            C1RXF5SIDbits.SID = sid; 
        break;
    	default:C1RXF0SID = 0;
        	C1RXF0SIDbits.SID = sid; 
        break;
	}
}

/*************************************************************************
* Function Name : CANSetMask
* Description   : This function sets the values for the acceptance 
*                 filter masks for standard length identifiers.

* Parameters    : uint16_t: mask_no:  (Mask number 0-1)
*                 uint16_t: mask:     (Mask value)        
**************************************************************************/

//  There are only 2 possible masks.  One for RXB0 and thte other for RXB1
void CANSetMask(uint8_t mask_no, uint16_t mask) { 
	// Here's how the mask thing works:
	// You have your list of SIDs elsewhere saved. (your list of 6 filters).
	// If you want to test for rejection of bits agreeing with the bits in your 6 filters, then set 
	// the mask bit to 1.  If you don't care, then set it to 0.
	// EX: mask = 0b11000011100
	//     SIDs that you have saved above = 0b00000000111
	//     		  							0b11111100000
	// This message id comes in:			0b01010101010
	// The mask forces the CAN module to check bit 15, 14, 4, 3, and 2 to see if the incoming message matches
	// at least one of the saved SIDs.  It fails.  haha.
  	switch(mask_no) {
    	case 0: C1RXM0SID = 0x0001;  // FRM Pg. 646.  Match only standard length identifiers.  Don't match extended.
        	C1RXM0SIDbits.SID = mask; 
        break;
    	case 1: C1RXM1SID = 0x0001;
        	C1RXM1SIDbits.SID = mask; 
        break;
    	default:C1RXM0SID = 0x0001;
        	C1RXM0SIDbits.SID = mask; 
        break;
	}
}

/*************************************************************************
* Function Name : CANSetPriority
* Description   : This function sets the message transmission  
*                 priority bits.

* Parameters    : uint16_t: buffer:   (Transmit buffer number 0-3) 
                  uint16_t: priority: (Transmit priority 0-3)        
**************************************************************************/

void CANSetPriority(uint8_t buffer, uint8_t priority)
  {
  //priority = (priority > 3U) ? 3U : priority;
	if (priority > 0b11)
		priority = 0b11;

	switch(buffer) {
    	case 0: C1TX0CONbits.TXPRI = priority;  // FRM Pg. 637
        break;
    	case 1: C1TX1CONbits.TXPRI = priority;
        break;
    	case 2: C1TX2CONbits.TXPRI = priority;
        break;
    	default:
        break;
	}
}  
  
/*********************************************************************
* Function Name : CANSendMessage 
* Description   : This function writes the message identifier(SID), 
                  writes the data to be transmitted into the Transmit buffer
*                 and sets the corresponding Transmit request bit.

* Parameters    : Pointer to structure CANMSG defined in can.h
*********************************************************************/ 

//int CANSendMessage(const CANMSG *message) { 
int CANSendMessage(CANMSG *message) { 
	// This fails if there is no free buffer.
  	uint8_t buffer;
	// Pg. 638 in FRM.  The SID is broken up into two pieces.  It's stupid.
	// The SID is 11 bits long.  Enough for 2048 possible message types.
  	uint16_t ix = ((message->id & 0b11111000000) << 5) | ((message->id & 0b111111) << 2); 
	
  	buffer = GetTXBuffer();
	if (buffer == TRANSMIT_BUFFERS_FULL)
		return 0;

	// This loads the identifier, and sets SRR to 0, and TXIDE to 0.  So, standard identifier, and no remote request.
 	switch(buffer) {
    	case 0: C1TX0SID = ix;
        break;
    	case 1: C1TX1SID = ix;
        break;
    	case 2: C1TX2SID = ix;
        break;
    	default:C1TX0SID = ix;
        break;
	}

	// load all the bytes of the data into the transmit buffer.	
  	for(ix = 0; ix < message->length; ix++) {
    	switch(buffer) {
      		case 0: *((uint8_t *)&C1TX0B1+ix)= message->data[ix];
            break;
      		case 1: *((uint8_t *)&C1TX1B1+ix)= message->data[ix];
            break;
      		case 2: *((uint8_t *)&C1TX2B1+ix)= message->data[ix];
            break;
      		default:*((uint8_t *)&C1TX0B1+ix)= message->data[ix];
            break;
      	}
	}
	// Transmit buffer data length control
  	switch(buffer) {
    	case 0: 
			//C1TX0DLC = 0b0000000110000000; //0b0000 0001 1000 0000; //0x0180;
			C1TX0DLC = 0; // The above initialization was wrong according to FRM pg. 639.
			// You want TXRB reserved bits to be set to 0b00, not 0b11.
        	C1TX0DLCbits.DLC = message->length;
        	C1TX0CONbits.TXREQ = 1;
        break;
    	case 1: 
			C1TX1DLC = 0;
        	C1TX1DLCbits.DLC = message->length;
        	C1TX1CONbits.TXREQ = 1;
        break;
    	case 2: 
			C1TX2DLC = 0;
        	C1TX2DLCbits.DLC = message->length;
        	C1TX2CONbits.TXREQ = 1;
		break;
    	default:
		break;
	}
	return 1;
}

/*************************************************************************
* Function Name : CANReceiveMessage
* Description   : If a message has been received, read the data from 
                  the receive buffer into the structure CANMSG
                  and clear the RXFUL bit.

* Parameters    : Pointer to structure CANMSG defined in can.h
* Returns       : True if message received else False.
**************************************************************************/

int CANReceiveMessage(CANMSG *message) {
	uint8_t ix, buffer = 0xff;

	if(C1RX0CONbits.RXFUL){	// FRM Pg. 640.  If the receive buffer contains a valid message...
		buffer = 0;
	}
	else if(C1RX1CONbits.RXFUL){ // FRM Pg. 641.  Does the receive buffer contain a valid message?
    	buffer = 1;
    }
  	if(buffer != 0xff) {
    	switch(buffer) {
			// FRM Pg. 642.  Save the message SID.
			// Assume it's standard length of 11 bits.
			// Also, assume no remote transfer request occurred.
      		case 0: message->id = (C1RX0SID >> 2) & 0b11111111111; // 0x07ff;
            	message->length = C1RX0DLCbits.DLC; // Length of message.
            break;
      		case 1: message->id = (C1RX1SID >> 2) & 0b11111111111; // 0x07ff;
              	message->length = C1RX1DLCbits.DLC; // Length of message.
            break;
      		default:
            break;
      	}  

		// Save the message to the message data.
    	for(ix = 0; ix < message->length; ix++) {
      		switch(buffer) {
        		case 0: message->data[ix] = *((uint8_t *)&C1RX0B1 + ix);
                break;
        		case 1: message->data[ix] = *((uint8_t *)&C1RX1B1 + ix);
                break;
        		default:
              	break;
        	}
      	}

		// Now, clear the flag for the receive buffer that had the message.
    	switch(buffer) {
      		case 0: C1RX0CONbits.RXFUL = 0;
            break;
      		case 1: C1RX1CONbits.RXFUL = 0;
            break;
      		default:
            break;  
      	}
	} 
	if (buffer != 0xff)
		return 1;
	return 0;
}

/*************************************************************************
* Function Name : getTxbuffer
* Description   : Returns an available empty transmit buffer.

* Returns       : Transmit buffer number 0-2.
**************************************************************************/

//static 
uint8_t GetTXBuffer(void) {
	uint8_t buffer = 0;

	if (C1TX0CONbits.TXREQ == 0) {  // FRM Pg. 637.  If TXREQ == 0, then that buffer is empty.
		buffer = 0;
	}
  	else if(C1TX1CONbits.TXREQ == 0){
    	buffer = 1;
	}
  	else if(C1TX2CONbits.TXREQ == 0){
    	buffer = 2;
	}
	else {
		buffer = TRANSMIT_BUFFERS_FULL;  // all buffers full!
	}
	return buffer;
}

void InitCanbus() {
// The controller and charger received every stinking message from the 6 nodes.  The nodes keep sending the voltages
// over and over and over!  And send errors too.  So, it needs to have masks that accept everything!
// The charger and controller need to talk to each other I think.
// So, I'll set all the filters to 0b00000000000, and then all the SIDs will be nonzero.
	uint8_t i;

	CANInit();
	for (i = 0; i < 6; i++) {
		CANSetFilter(i, 0);  // 
	}

	CANSetMask(0, 0b00000000000);  // None of the bits have to match any of the filters.  So, accept everything.
	CANSetMask(1, 0b00000000000);

	CANSetPriority(0, 0b11);  // set each buffer to the highest priority transmission.  The bosses are important!  haha
	CANSetPriority(1, 0b11);
	CANSetPriority(2, 0b11);

	CANConfig();
}

//determine whether there are messages waiting or not
int canbus_kbhit() {
	if(C1RX0CONbits.RXFUL || C1RX1CONbits.RXFUL) return 1;
	return 0;
}

//we don't listen for nothin' yet but go ahead and get them from the buffer anyway
void canbus_receive(void) {
	CANMSG thisMsg;
	if (canbus_kbhit()) {
		CANReceiveMessage(&thisMsg);
	}
}

//send out some debugging messages
//all of this is subject to change at any time and for any reason
//don't take the protocol definitions to be steady or constant. Any time
//you want some data output just change this. Until, of course, we get
//closer to the end then we should get something finalized.
void canbus_send_status(void) {
	CANMSG sendMsg;
	signed int temp;
	static int which = 0;
	switch (which) {
	case 0:
		sendMsg.id = 0x555 + 0x8;
		sendMsg.length = 6;
		sendMsg.data[0] = i_alpha & 0x00FF;
		sendMsg.data[1] = (i_alpha & 0xFF00) >> 8;
		sendMsg.data[2] = i_b & 0x00FF;
		sendMsg.data[3] = (i_b & 0xFF00) >> 8;
		//temp = -i_alpha - i_b;
		temp = 0;
		sendMsg.data[4] = temp & 0x00FF;
		sendMsg.data[5] = (temp & 0xFF00) >> 8;
		break;
	case 1:
		sendMsg.id = 0x555 + 0xA;
		sendMsg.length = 8;
		//dont have RMS current yet so just send i_alpha as a placeholder)
		sendMsg.data[0] = i_alpha & 0x00FF;
		sendMsg.data[1] = (i_alpha & 0xFF00) >> 8;
		temp = (hv_raw_input * volt_gain);
		sendMsg.data[2] = temp & 0x00FF;
		sendMsg.data[3] = (temp & 0xFF00) >> 8;
		sendMsg.data[4] = (unsigned char)(throttle_raw >> 2);
		//dont have a second throttle input yet so...
		sendMsg.data[5] = (unsigned char)(throttle_raw >> 2);
		sendMsg.data[6] = 0; //don't know motor temp yet either... I'm seeing a trend
		sendMsg.data[7] = 0; //also dont have controller temp
		break;
	case 2:
		sendMsg.id = 0x555 + 0xC;
		sendMsg.length = 3;
		temp = hertz * 60; //hertz * 60 = RPM
		sendMsg.data[0] = temp * 0x00FF; //motor RPM. Indirectly calculated from vhz stuff
		sendMsg.data[1] = (temp & 0xFF00) >> 8; 
		sendMsg.data[2] = 0; //limiting cause. dont have it yet
		break;
	default:
		sendMsg.id = 0x555 + 0x8;
		sendMsg.length = 6;
		sendMsg.data[0] = i_alpha & 0x00FF;
		sendMsg.data[1] = (i_alpha & 0xFF00) >> 8;
		sendMsg.data[2] = i_b & 0x00FF;
		sendMsg.data[3] = (i_b & 0xFF00) >> 8;
		//temp = -i_alpha - i_b;
		temp = 0;
		sendMsg.data[4] = temp & 0x00FF;
		sendMsg.data[5] = (temp & 0xFF00) >> 8;
		break;
	}
	CANSendMessage(&sendMsg);
	which = (which + 1) % 3;
}

